#!/usr/bin/env python3
"""
@Author: wujin_xueyk\n
@Date:   2022-01-01\n
@Function: Geodetic tools like coordinate convert, distance calculator, yaw calculator, etc.\n
@Refs:\n
1. See other similar libs like geopy\n
"""

import sys
import time
import math
from .geographic_coordinate_conversion import *

##########################################################################
# Calculate distance
##########################################################################


def calculate_haversin_distance(gps1, gps2):
    """
    计算大地坐标系（WGS84）下两个经纬度坐标点之间的距离（地表弧长）。\n
    Calculate Haversine distance(in meter) between two global position(WGS84).\n

    Inputs:
        - gps1: a tuple, first position
        - gps2: a tuple, secend position
    Outputs:
        distance: a value, haversine distance(in meter) between the position
    """
    R = 6371000.0
    lat1 = math.radians(gps1[0])
    lon1 = math.radians(gps1[1])
    lat2 = math.radians(gps2[0])
    lon2 = math.radians(gps2[1])
    tmp = math.sin((lat1-lat2)/2)**2 + math.cos(lat1) * \
        math.cos(lat2)*math.sin((lon1-lon2)/2)**2
    # dis = 2*R*math.asin(math.sqrt(tmp))
    dis = 2*R*math.atan2(math.sqrt(tmp), math.sqrt(1-tmp))
    return dis


def calculate_3d_distance(gps1, gps2):
    """
    计算大地坐标系（WGS84）下两个经纬度坐标点之间的三维空间中直线距离.\n

    Inputs:
        - gps1: a tuple, first position
        - gps2: a tuple, secend position
    Outputs:
        distance: a value, 3d distance(in meter) between the position
    """
    pos1 = cc_geodetic2ecef(gps1)
    pos2 = cc_geodetic2ecef(gps2)
    print(pos1)
    print(pos2)
    dis = math.sqrt((pos2[0]-pos1[0])**2 +
                    (pos2[1]-pos1[1])**2 + (pos2[2]-pos1[2])**2)
    return dis
##########################################################################
# Convert yaw
##########################################################################


def convert_yaw_NED2baselink(yaw):
    """
    偏航角转换，将在NED坐标系下的偏航角转成在baselink坐标系下的偏航角。\n
    Convert yaw represented in NED frame to baselink(NWU) frame.\n

    Inputs:
        - yaw: yaw in NED frame
    Outputs:
        yaw in baselink frame
    """
    yaw = -yaw
    return yaw


def convert_yaw_NED2ENU(yaw):
    """
    偏航角转换，将在NED坐标系下的偏航角转成在ENU坐标系下的偏航角。\n
    Convert yaw represented in NED frame to baselink(NWU) frame.\n

    Inputs:
        - yaw: yaw in NED frame
    Outputs:
        yaw in baselink frame
    """
    yaw = -yaw
    yaw += math.pi/2
    if yaw > math.pi:
        yaw = yaw - math.pi * 2
    return yaw


def calculate_delta_yaw(yaw_base, yaw):
    """
    以一个偏航角为基准，计算另一个偏航角与其的相对偏移，这里的两个偏航角默认都是表示在NED坐标系下。\n
    如果yaw在yaw_base右侧，则返回值>0；\n
    如果yaw在yaw_base左侧，则返回值>0；\n
    Get the angle between yaw_base and yaw, take yaw_base as 0, get relative angle of yaw\n
    delta_yaw>0: if yaw at right of yaw_base\n
    delta_yaw<0: if yaw at left of yaw_base\n

    Inputs:
        - yaw_base: base reference yaw
        - yaw: activate yaw
    Outputs:
        delta yaw angle
    """
    '''
    # delta_yaw belong to [-180, 180]
    #         yaw_base  yaw
    #             ^      ^
    #              \    /
    #               \ a/
    #             -  \/  +
    '''
    yaw_delta = yaw - yaw_base
    yaw_rel = 0
    if yaw_delta < 0 and abs(yaw_delta) < math.pi:
        yaw_rel = yaw_delta
    elif yaw_delta < 0 and abs(yaw_delta) > math.pi:
        yaw_rel = yaw_delta + 2*math.pi
    elif yaw_delta > 0 and abs(yaw_delta) > math.pi:
        yaw_rel = yaw_delta - 2*math.pi
    elif yaw_delta > 0 and abs(yaw_delta) < math.pi:
        yaw_rel = yaw_delta
    return yaw_rel


def calculate_yaw_accordingto_two_global_position(gps_base, gps, frame='ned'):
    """
    以一个经纬度坐标作为基准，根据另外一个经纬度坐标计算表示在NED/ENU/Baselink(NWU)坐标系下的偏航角.\n
    Calculate yaw of the desired gps in NED/ENU/Baselink(NWU) frame.\n
    The angle a belong to [-180, 180].\n

    Inputs:
        - gps_base: base reference gps
        - gps: activate gps
        - frame: ned, enu or baselink
    Outputs:
        yaw angle in specified frame.
    """
    lat1 = math.radians(gps_base[0])
    lon1 = math.radians(gps_base[1])
    lat2 = math.radians(gps[0])
    lon2 = math.radians(gps[1])

    pi_half = math.pi / 2.0

    cos_c = math.cos(pi_half-lat2) * math.cos(pi_half-lat1) + \
        math.sin(pi_half-lat2)*math.sin(pi_half-lat1)*math.cos(lon2-lon1)
    sin_c = math.sqrt(1 - cos_c**2)
    sin_yaw = (math.sin(pi_half - lat2) * math.sin(lon2 - lon1)) / sin_c
    sin_yaw = sin_yaw if sin_yaw < 1.0 else 1.0
    sin_yaw = sin_yaw if sin_yaw > -1.0 else -1.0
    yaw = math.asin(sin_yaw)

    # first quadrant(1)
    if lat2 > lat1 and lon2 >= lon1:
        yaw = yaw
    # second quadrant(2)
    elif lat2 >= lat1 and lon2 < lon1:
        yaw = yaw
    # third quadrant(3)
    elif lat2 < lat1 and lon2 < lon1:
        yaw = -(math.pi + yaw)
    # fourth quadrant(4)
    elif lat2 < lat1 and lon2 >= lon1:
        yaw = math.pi - yaw

    # NED frame
    #             0 North
    #             ^    gps
    #             |   ^
    #             |a /
    #             | /
    # -pi/2_______|/________ pi/2
    #             |gps_base
    #             |
    #         -pi | pi
    # baselink(NWU) frame
    #             0 North
    #             ^    gps
    #             |   ^
    #             |a /
    #             | /
    # pi/2 _______|/________ -pi/2
    #             |gps_base
    #             |
    #          pi | -pi
    # ENU frame
    #        pi/2 North
    #             ^    gps
    #             |   ^
    #             |a /
    #     pi      | /
    #     ________|/________ 0
    #             |gps_base
    #     -pi     |
    #             |
    #           -pi/2
    if frame == 'ned':
        yaw = yaw
    elif frame == 'baselink':
        yaw = -yaw
    elif frame == 'enu':
        yaw = -yaw
        yaw += math.pi/2
        if yaw > math.pi:
            yaw = yaw - math.pi * 2
    return yaw


def calculate_yaw_accordingto_two_local_position(position_base, position, frame='ned'):
    """
    以一个局部坐标作为基准，根据另外一个局部坐标计算表示在NED/ENU/Baselink(NWU)坐标系下的偏航角.\n
    Calculate yaw of the desired position in NED/ENU/Baselink(NWU) frame.\n
    The yaw angle a belong to [-180, 180].\n

    Inputs:
        - position_base: base reference position
        - position: activate position
        - frame: ned, enu or baselink
    Outputs:
        yaw angle in specified frame.
    """
    yaw = math.atan2(lp2[0]-lp1[0], lp2[1]-lp1[1])
    # NED frame
    #             0   North(local_x+)
    #             ^    lp2
    #             |   ^
    #             |a /
    #             | /              (local_y+)
    # -pi/2_______|/________ pi/2 East
    #             | lp1
    #             |
    #         -pi | pi
    # Baselink(NWU) frame
    #             0   North(local_x+)
    #             ^    lp2
    #             |   ^
    # (local_y+)  |a /
    # West        | /
    # pi/2 _______|/________ -pi/2
    #             | lp1
    #             |
    #          pi | -pi
    # ENU frame
    #           pi/2   North(local_y+)
    #             ^    lp2
    #             |   ^
    #             |a /
    #             | /
    #  pi  _______|/________ 0 East(local_x+)
    # -pi         | lp1
    #             |
    #             |
    #           -pi/2
    if frame == 'ned':
        yaw = yaw
    elif frame == 'baselink':
        yaw = convert_yaw_NED2baselink(yaw)
    elif frame == 'enu':
        yaw = convert_yaw_NED2ENU(yaw)
    return yaw

##########################################################################
# Calculate the second gps location
##########################################################################


def calculate_second_gps_ned(gps_base, distance, angle):
    """
    以一个经纬度坐标作为基准，根据方向和距离，计算满足条件的另一个经纬度坐标.\n
    Calculate a second gps base on the known gps, distance between them and the angle.\n
    angle: defined in ned frame.\n

    Inputs:
        - gps_base: base reference gps
        - distance: the distance to unknown gps
        - angle: the distance to unknown gps
    Outputs:
        the unknown gps satisfied
    """
    R = 6371000.0
    lat1 = math.radians(gps_base[0])
    lon1 = math.radians(gps_base[1])
    pi_half = math.pi / 2.0
    c = distance / R

    a = math.acos(math.cos(pi_half - lat1)*math.cos(c) +
                  math.sin(pi_half - lat1)*math.sin(c) * math.cos(angle))
    C = math.asin(math.sin(c)*math.sin(angle) / math.sin(a))

    lat2 = math.degrees(pi_half - a)
    lon2 = math.degrees(lon1 + C)

    return (lat2, lon2)

##########################################################################
# Convert position between global, local, user frame
##########################################################################


def convert_position_global2local(origin_local, global_positions):
    """
    以一个经纬度坐标点作为局部坐标系的原点，计算大地坐标系下经纬度坐标在该局部坐标系下的局部坐标。\n
    Convert position in global frame(WGS84) to position in local frame(NED).\n
    local frame is defined by PX4, the origin is home position.\n

    Inputs:
        - origin_local     : origin of local frame represent in gps, (lat, lon, alt)
        - global_positions : the gps point or a list of gps points, (lat1, lon1, alt1) or [(lat1, lon1, alt1), (lat2, lon2, alt2), ...]
    Outputs:
        local positions
    """
    origin_local_lat = origin_local[0]
    origin_local_lon = origin_local[1]

    def converter(g_pos):
        lat, lon = g_pos[0], g_pos[1]
        local_x = calculate_haversin_distance(
            origin_local, (lat, origin_local_lon))
        if lat < origin_local_lat:
            local_x = -local_x

        local_y = calculate_haversin_distance(
            origin_local, (origin_local_lat, lon))
        if lon < origin_local_lon:
            local_y = -local_y
        l_pos_xy = (local_x, local_y)
        l_pos = l_pos_xy
        if len(origin_local) == 3 and len(g_pos) == 3:
            origin_local_alt = origin_local[2]
            alt = g_pos[2]
            l_pos_z = alt - origin_local_alt
            # LocalFrame is NED!
            l_pos_z = -l_pos_z
            l_pos = (local_x, local_y, l_pos_z)
        return l_pos

    if isinstance(global_positions, list):
        local_positions = []
        for i, g_pos in enumerate(global_positions):
            l_pos = converter(g_pos)
            local_positions.append(l_pos)
    else:
        local_positions = converter(global_positions)
    return local_positions


def convert_position_local2global(origin_local, local_positions):
    """
    以一个经纬度坐标点作为局部坐标系的原点，计算在该局部坐标系下的局部坐标对应的大地坐标系下经纬度坐标。\n
    Convert position in local frame(NED) to position in global frame(WGS84).\n
    local frame is defined by PX4, the origin is home position.\n

    Inputs:
        - origin_local    : origin of local frame represent in gps, (lat, lon, alt)
        - local_positions : the local point or a list of local points,  (x1, y1, z1) or [(x1, y1, z1), (x2, y2, z2), ...]
    Outputs:
        global positions
    """
    origin_local_lat = origin_local[0]
    origin_local_lon = origin_local[1]

    def converter(l_pos):
        l_x = l_pos[0]
        l_y = l_pos[1]
        lat = calculate_second_gps_ned(origin_local, l_x, 0)[
            0] if l_x >= 0 else calculate_second_gps_ned(origin_local, -l_x, math.pi)[0]
        lon = calculate_second_gps_ned(origin_local, l_y, math.pi/2)[
            1] if l_y >= 0 else calculate_second_gps_ned(origin_local, -l_y, -math.pi/2)[1]
        g_pos = (lat, lon)
        if len(origin_local) == 3 and len(l_pos) == 3:
            origin_local_alt = origin_local[2]
            l_z = l_pos[2]
            alt = origin_local_alt + l_z
            g_pos = (lat, lon, alt)
        return g_pos

    if isinstance(local_positions, list):
        global_positions = []
        for i, l_pos in enumerate(local_positions):
            g_pos = converter(l_pos)
            global_positions.append(g_pos)
    else:
        global_positions = converter(local_positions)
    return global_positions


def convert_position_global2user(origin_user, yaw, global_positions):
    """
    以一个经纬度坐标为原点并指定偏转角度来定义用户坐标系，将大地坐标系下经纬度坐标转换成用户坐标系下的位置坐标。\n
    Convert position in global frame(WGS84) to position in user frame(FRD).\n
    user frame is defined by user, user should set the origin(lat, lon, alt) and yaw.\n

    Inputs:
        - origin_user      : origin of user frame represent in gps, (lat, lon, alt)
        - yaw              : yaw angle of rotate from global frame to user frame
        - global_positions : the gps point or a list of gps points, (lat1, lon1, alt1) or [(lat1, lon1, alt1), (lat2, lon2, alt2), ...]
    Outputs:
        user positions
    """
    def rotate(pos_e):
        x_e, y_e = pos_e[0], pos_e[1]
        pos_b = (math.cos(yaw) * x_e + math.sin(yaw) * y_e,
                 -math.sin(yaw) * x_e + math.cos(yaw) * y_e)
        if len(pos_e) == 3:
            pos_b = (pos_b[0], pos_b[1], pos_e[2])
        return pos_b

    local_positions = convert_position_global2local(
        origin_user, global_positions)
    if isinstance(local_positions, list):
        user_positions = []
        for i, l_pos in enumerate(local_positions):
            u_pos = rotate(l_pos)
            user_positions.append(u_pos)
    else:
        user_positions = rotate(local_positions)
    return user_positions


def convert_position_user2global(origin_user, yaw, user_positions):
    """
    以一个经纬度坐标为原点并指定偏转角度来定义用户坐标系，将用户坐标系下的位置坐标转换成大地坐标系下经纬度坐标。\n
    Convert position in global frame(WGS84) to position in user frame(FRD).\n
    user frame is defined by user, user should set the origin(lat, lon, alt) and yaw.\n

    Inputs:
        - origin_user     : origin of user frame represent in gps, (lat, lon, alt)
        - yaw             : yaw angle of rotate from global frame to user frame
        - user_positions  : the user point or a list of user points, (x1, y1, z1) or [(x1, y1, z1), (x2, y2, z2), ...]
    Outputs:
        - global positions
    """
    def rotate(pos_b):
        x_b, y_b = pos_b[0], pos_b[1]
        pos_e = (math.cos(yaw) * x_b - math.sin(yaw) * y_b,
                 math.sin(yaw) * x_b + math.cos(yaw) * y_b)
        if len(pos_b) == 3:
            pos_e = (pos_e[0], pos_e[1], pos_b[2])
        return pos_e

    if isinstance(user_positions, list):
        local_positions = []
        for i, u_pos in enumerate(user_positions):
            l_pos = rotate(u_pos)
            local_positions.append(l_pos)
    else:
        local_positions = rotate(user_positions)

    global_positions = convert_position_local2global(
        origin_user, local_positions)
    return global_positions


def convert_position_user2local(origin_local, origin_user, yaw, user_positions):
    """
    将用户坐标系下的位置坐标转换成局部坐标系下的位置坐标。\n
    Convert position in user frame(FRD) to position in local frame(NED).\n
    local frame is defined by PX4, the origin is home position.\n
    user frame is defined by user, user should set the origin(lat, lon, alt) and yaw.\n

    Inputs:
        - origin_local    : origin of local frame represent in gps, (lat, lon, alt)
        - origin_user     : origin of user frame represent in gps, (lat, lon, alt)
        - yaw             : yaw angle of rotate from global frame to user frame
        - user_positions  : the user point or a list of user points, (x1, y1, z1) or [(x1, y1, z1), (x2, y2, z2), ...]
    Outputs:
        local positions
    """
    l_o_lat, l_o_lon = origin_local[0], origin_local[1]
    u_o_lat, u_o_lon = origin_user[0],  origin_user[1]

    offset_x = calculate_haversin_distance(origin_local, (u_o_lat, l_o_lon))
    if u_o_lat < l_o_lat:
        offset_x = - offset_x
    offset_y = calculate_haversin_distance(origin_local, (l_o_lat, u_o_lon))
    if u_o_lon < l_o_lon:
        offset_y = - offset_y

    offset_z = None
    if len(origin_local) == 3 and len(origin_user) == 3:
        offset_z = origin_user[2] - origin_local[2]

    def rotate(pos_b):
        x_b, y_b = pos_b[0], pos_b[1]
        pos_e = (math.cos(yaw) * x_b - math.sin(yaw) * y_b + offset_x,
                 math.sin(yaw) * x_b + math.cos(yaw) * y_b + offset_y)
        if offset_z is not None and len(pos_b) == 3:
            pos_e = (pos_e[0], pos_e[1], pos_b[2] + offset_z)
        return pos_e

    if isinstance(user_positions, list):
        local_positions = []
        for i, u_pos in enumerate(user_positions):
            l_pos = rotate(u_pos)
            local_positions.append(l_pos)
    else:
        local_positions = rotate(user_positions)
    return local_positions


def convert_position_local2user(origin_local, origin_user, yaw, local_positions):
    """
    将局部坐标系下的位置坐标转换成用户坐标系下的位置坐标。\n
    Convert position in local frame(NED) to position in user frame(FRD).\n
    local frame is defined by PX4, the origin is home position.\n
    user frame is defined by user, user should set the origin(lat, lon, alt) and yaw.\n

    Inputs:
        - origin_local    : origin of local frame represent in gps, (lat, lon, alt)
        - origin_user     : origin of user frame represent in gps, (lat, lon, alt)
        - yaw             : yaw angle of rotate from global frame to user frame
        - local_positions : the local point or a list of local points,  (x1, y1, z1) or [(x1, y1, z1), (x2, y2, z2), ...]
    Outputs:
        user positions.
    """
    l_o_lat, l_o_lon = origin_local[0], origin_local[1]
    u_o_lat, u_o_lon = origin_user[0],  origin_user[1]

    offset_x = calculate_haversin_distance(origin_local, (u_o_lat, l_o_lon))
    if u_o_lat < l_o_lat:
        offset_x = - offset_x
    offset_y = calculate_haversin_distance(origin_local, (l_o_lat, u_o_lon))
    if u_o_lon < l_o_lon:
        offset_y = - offset_y

    offset_z = None
    if len(origin_local) == 3 and len(origin_user) == 3:
        offset_z = origin_user[2] - origin_local[2]

    def rotate(pos_b):
        x_b, y_b = pos_b[0], pos_b[1]
        pos_e = (math.cos(yaw) * x_b + math.sin(yaw) * y_b - offset_x,
                 -math.sin(yaw) * x_b + math.cos(yaw) * y_b - offset_y)
        if offset_z is not None and len(pos_b) == 3:
            pos_e = (pos_e[0], pos_e[1], pos_b[2] - offset_z)
        return pos_e

    if isinstance(local_positions, list):
        user_positions = []
        for i, l_pos in enumerate(local_positions):
            u_pos = rotate(l_pos)
            user_positions.append(u_pos)
    else:
        user_positions = rotate(local_positions)
    return user_positions


def convert_vector_user2body(v_user, user_yaw, body_yaw):
    """
    Convert vector in user frame to body frame.\n

    Inputs:
        - v_user  : 3x1 vector or vector list, for example v_user=(1,2,3) or v_user=[(1,2,3), (2,3,4), (4,5,6)...]
        - user_yaw: yaw angle of user frame relative to global frame, unit rad
        - body_yaw: yaw angle of body frame relative to global frame, unit rad

    Outputs:
        vector in body frame
    """
    # user_yaw  body_yaw
    #     ^      ^
    #      \ yaw/
    #       \  /
    #     -  \/  +
    yaw_delta = body_yaw - user_yaw
    yaw = 0
    if yaw_delta < 0 and abs(yaw_delta) < math.pi:
        yaw = yaw_delta
    elif yaw_delta < 0 and abs(yaw_delta) > math.pi:
        yaw = yaw_delta + 2*math.pi
    elif yaw_delta > 0 and abs(yaw_delta) > math.pi:
        yaw = yaw_delta - 2*math.pi
    elif yaw_delta > 0 and abs(yaw_delta) < math.pi:
        yaw = yaw_delta

    def converter(vec, yaw):
        x = math.cos(yaw)*vec[0] + math.sin(yaw)*vec[1]
        y = -math.sin(yaw)*vec[0] + math.cos(yaw)*vec[1]
        z = vec[2]
        return (x, y, z)
    if isinstance(v_user, list):
        v_body = []
        for i, vec in enumerate(v_user):
            v_b = converter(vec, yaw)
            v_body.append(v_b)
    else:
        v_body = converter(v_user, yaw)
    return v_body


def convert_vector_body2user(v_body, body_yaw, user_yaw):
    """
    Convert vector in body frame to user frame.\n

    Inputs:
        - v_body  : 3x1 vector or vector list, for example v_body=(1,2,3) or v_body=[(1,2,3), (2,3,4), (4,5,6)...]
        - body_yaw: yaw angle of body frame relative to global frame, unit rad
        - user_yaw: yaw angle of user frame relative to global frame, unit rad

    Outputs:
        vector in user frame.
    """
    # user_yaw  body_yaw
    #     ^      ^
    #      \ yaw/
    #       \  /
    #     -  \/  +
    yaw_delta = body_yaw - user_yaw
    yaw = 0
    if yaw_delta < 0 and abs(yaw_delta) < math.pi:
        yaw = yaw_delta
    elif yaw_delta < 0 and abs(yaw_delta) > math.pi:
        yaw = yaw_delta + 2*math.pi
    elif yaw_delta > 0 and abs(yaw_delta) > math.pi:
        yaw = yaw_delta - 2*math.pi
    elif yaw_delta > 0 and abs(yaw_delta) < math.pi:
        yaw = yaw_delta

    def converter(vec, yaw):
        x = math.cos(yaw)*vec[0] - math.sin(yaw)*vec[1]
        y = math.sin(yaw)*vec[0] + math.cos(yaw)*vec[1]
        z = vec[2]
        return (x, y, z)
    if isinstance(v_body, list):
        v_user = []
        for i, vec in enumerate(v_body):
            v_u = converter(vec, yaw)
            v_user.append(v_u)
    else:
        v_user = converter(v_body, yaw)
    return v_user


def convert_vector_global2user(v_global, user_yaw):
    """
    Convert vector in global frame to user frame.\n

    Inputs:
        - v_global: 3x1 vector or vector list, for example v_global=(1,2,3) or v_global=[(1,2,3), (2,3,4), (4,5,6)...]
        - user_yaw: yaw angle of user frame relative to global frame, unit rad

    Outputs:
        vector in user frame
    """
    yaw = user_yaw

    def converter(vec, yaw):
        x = math.cos(yaw)*vec[0] + math.sin(yaw)*vec[1]
        y = -math.sin(yaw)*vec[0] + math.cos(yaw)*vec[1]
        z = vec[2]
        return (x, y, z)
    if isinstance(v_global, list):
        v_user = []
        for i, vec in enumerate(v_global):
            v_b = converter(vec, yaw)
            v_user.append(v_b)
    else:
        v_user = converter(v_global, yaw)
    return v_user


def convert_vector_user2global(v_user, user_yaw):
    """
    Convert vector in user frame to global frame.\n

    Inputs:
        - v_user: 3x1 vector or vector list, for example v_user=(1,2,3) or v_user=[(1,2,3), (2,3,4), (4,5,6)...]
        - user_yaw: yaw angle of user frame relative to global frame, unit rad

    Outputs:
        vector in global frame
    """
    yaw = user_yaw

    def converter(vec, yaw):
        x = math.cos(yaw)*vec[0] - math.sin(yaw)*vec[1]
        y = math.sin(yaw)*vec[0] + math.cos(yaw)*vec[1]
        z = -vec[2]
        return (x, y, z)
    if isinstance(v_user, list):
        v_global = []
        for i, vec in enumerate(v_user):
            v_b = converter(vec, yaw)
            v_global.append(v_b)
    else:
        v_global = converter(v_user, yaw)
    return v_global


def test_distance():
    origin = (30.7295, 103.9645, 100)
    gps = (30.7295, 103.9643, 200)

    dis1 = calculate_haversin_distance(origin, gps)
    dis2 = calculate_3d_distance(origin, gps)

    print(dis1, dis2)

    pos = cc_geodetic2enu(origin, gps)
    print(f"pos={pos}")


def test_delta_yaw():
    yaw_base = math.radians(100)
    yaw = math.radians(0)


def test_yaw():
    origin = (30.7295, 103.9645, 100)
    gps = (30.7293, 103.9643, 100)
    yaw_ned = calculate_yaw_accordingto_two_global_position(
        origin, gps, frame='ned')
    print(f'yaw_ned={math.degrees(yaw_ned)}')
    yaw_enu = calculate_yaw_accordingto_two_global_position(
        origin, gps, frame='enu')
    print(f'yaw_enu={math.degrees(yaw_enu)}')
    yaw_baselink = calculate_yaw_accordingto_two_global_position(
        origin, gps, frame='baselink')
    print(f'yaw_baselink={math.degrees(yaw_baselink)}')


def test_local_global():
    origin = (30.7295, 103.9645, 100)
    global_positions = [(30.7298, 103.9645, 105),
                        (30.7298, 103.9648, 106), (30.7295, 103.9648, 107)]
    # global_positions = (30.7290, 103.9645, 105)
    # local_positions = [(0, 0, 5), (30, 0, 6), (-30, -30, 7)]

    # test local and global
    local_positions = convert_position_global2local(origin, global_positions)
    print(local_positions)
    global_positions = convert_position_local2global(origin, local_positions)
    print(global_positions)


def test_user_global():
    origin = (30.7295, 103.9645, 100)
    global_positions = [(30.7290, 103.9645, 105),
                        (30.7295, 103.9645, 106), (30.7295, 103.9636, 107)]
    # global_positions = (30.7290238, 103.9645190, 105)
    # user_positions = [(0.8228424578768936, 42.16141780709144, 5), (54.86357680649079, 42.91651914577137, 6), (55.24163955744543, -42.5055146197468, 7)]

    # test user and global
    user_positions = convert_position_global2user(
        origin, -90, global_positions)
    print(user_positions)
    global_positions = convert_position_user2global(
        origin, -90, user_positions)
    print(global_positions)


def test_user_local():
    origin_user = (30.7291164, 103.9642779)
    origin_local = (30.7291164, 103.9642779)
    print("-------------------------------------------------------------------")
    user_positions = (10, 12, 5)
    local_positions = convert_position_user2local(
        origin_local, origin_user, math.pi, user_positions)
    print(local_positions)
    user_positions = convert_position_local2user(
        origin_local, origin_user, math.pi, local_positions)
    print(user_positions)
    print("-------------------------------------------------------------------")


def test_vector():
    user_yaw = 0.0
    body_yaw = 0.3

    print("-------------------------------------------------------------------")
    print("user={}, body={}".format(user_yaw*180/math.pi, body_yaw*180/math.pi))
    print("-------------------------------------------------------------------")
    v_body = [(1, 1, 1), (4, 0, 6), (0, -3, 6)]
    v_user = convert_vector_body2user(v_body, body_yaw, user_yaw)
    print(v_user)
    print("-------------------------------------------------------------------")
    v_body = convert_vector_user2body(v_user, user_yaw, body_yaw)
    print(v_body)

    print("===================================================================")
    user_yaw = 0.2
    v_global = [(1, 1, 1), (4, 0, 6), (0, -3, 6)]
    v_user = convert_vector_global2user(v_global, user_yaw)
    print('v_user=', v_user)
    v_global = convert_vector_user2global(v_user, user_yaw)
    print('v_global=', v_global)


def main():
    # test_distance()
    # test_yaw()
    # test_local_global()
    # test_user_global()
    # test_user_local()
    test_vector()


if __name__ == '__main__':
    main()
